//
// Created by zhanglei on 2024/1/28.
//

#ifndef SLAM_IN_AUTO_DRIVING_ESKF_FILTER_H
#define SLAM_IN_AUTO_DRIVING_ESKF_FILTER_H
#include <deque>
#include "common/gnss.h"
#include "common/imu.h"
#include "common/odom.h"
#include "common/math_utils.h"
#include "common/nav_state.h"

namespace sad {
struct State {
    double timestamp;
    Eigen::Vector3d pose;     
    Eigen::Vector3d velocity;  
    Eigen::Matrix3d rot;   
    Eigen::Quaterniond quat;
    Eigen::Vector3d ba; 
    Eigen::Vector3d bg;
    Eigen::Vector3d gravity_;
    // Covariance.
    Eigen::Matrix<double, 18, 18> cov;
};

class EskFilter {
public:
    struct Options {
        Options() = default;
        /// IMU 测量与零偏参数
        double imu_dt_ = 0.01;  // IMU测量间隔
        double sqrt_dt = std::sqrt(imu_dt_);
        // NOTE IMU噪声项都为离散时间，不需要再乘dt，可以由初始化器指定IMU噪声
        double gyro_var_ = 1.7453e-04 * sqrt_dt;       // 陀螺测量标准差
        double acce_var_ = 5.8860e-03 * sqrt_dt;       // 加计测量标准差
        double bias_gyro_var_ = 1.0000e-05;  // 陀螺零偏游走标准差
        double bias_acce_var_ = 1.0000e-04;  // 加计零偏游走标准差

        /// 里程计参数
        double odom_var_ = 100000.0;
        double odom_span_ = 0.01;        // 里程计测量间隔
        double left_wheel_radius_ = 0.623479 / 2.0;   // 轮子半径
        double right_wheel_radius_ = 0.622806 / 2.0;   // 轮子半径
        double circle_pulse_ = 4096.0;  // 编码器每圈脉冲数

        /// RTK 观测参数
        double gnss_pos_noise_ = 0.1;                   // GNSS位置噪声
        double gnss_height_noise_ = 0.1;                // GNSS高度噪声
        double gnss_ang_noise_ = 50.0 * math::kDEG2RAD;  // GNSS旋转噪声

        /// 其他配置
        bool update_bias_gyro_ = true;  // 是否更新陀螺bias
        bool update_bias_acce_ = true;  // 是否更新加计bias
    };

    EskFilter();
    bool Predict(const IMU& imu);

    bool ObserveGps(const GNSS& gnss);
    bool ObserveSE3(const SE3& pose, double trans_noise = 0.1, double ang_noise = 1.0 * math::kDEG2RAD);
    
    bool ObservePose(const SE3& pose, double trans_noise = 0.1, double ang_noise = 1.0 * math::kDEG2RAD);
    bool ObserveQuaternion(const SE3& pose, double trans_noise = 0.1, double ang_noise = 1.0 * math::kDEG2RAD);

    bool ObserveWheelSpeed(const Odom& odom);

    /**
     * 设置初始条件
     * @param options 噪声项配置
     * @param init_bg 初始零偏 陀螺
     * @param init_ba 初始零偏 加计
     * @param gravity 重力
     */
    void SetInitialConditions(Options options, const Vec3d& init_bg, const Vec3d& init_ba, const Vec3d& gravity,
                              const Mat3d& init_rot) {
        options_ = options;
        state_.bg = init_bg;
        state_.ba = init_ba;
        state_.rot = init_rot;
        state_.quat = Eigen::Quaterniond(init_rot);
        state_.pose.setZero();
        state_.velocity.setZero();
        // Set covariance.
        state_.cov.setZero() = Eigen::Matrix<double, 18, 18>::Identity() * 1e-4;

        gravity_ = gravity;
    }

    NavStated GetNominalState() const { 
        return NavStated(current_time_, Sophus::SO3d(), state_.pose, state_.velocity, state_.bg, state_.ba); 
    }


    inline Eigen::Matrix3d SkewMatrix(const Eigen::Vector3d& v) {
        Eigen::Matrix3d w;
        w <<  0.,   -v(2),  v(1),
            v(2),  0.,   -v(0),
            -v(1),  v(0),  0.;

        return w;
    }
    /// Roderigous Tranformation
    Eigen::Matrix3d Exp(const Eigen::Vector3d &axis_angle);
    Eigen::Vector3d Log(const Eigen::Matrix3d& R);
private:

    Options options_;                 // 选项信息

    State state_;

    Vec3d gravity_;

    IMU last_imu_;

    double current_time_ = 0.0;  // 当前时间

    /// 标志位
    bool first_gnss_ = true;  // 是否为第一个gnss数据

    Eigen::Matrix3d Rwb; 

    Eigen::Vector3d twb;  
    GNSS gnss_;
    Odom last_odom_;
};

}

#endif